/*
 * @Author: wen.ligang
 * @Date: 2019-11-08 18:35:30
 * @LastEditTime: 2019-11-08 23:13:14
 * @LastEditors: Wen.ligang
 * @Description: In User Settings Edit
 * @FilePath: /cartographer_interface/src/file_read.cc
 */
#include "csvFileRead.h"
#include "stdio.h"
#include <iostream>
#include <string.h>
#include <vector>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <vector>
#include <fstream>  
#include<sstream>
using namespace std;

inline string csvFileRead::Trim(string &str)
{
    //str.find_first_not_of(" \t\r\n"),在字符串str中从索引0开始，返回首次不匹配"\t\r\n"的位置
    str.erase(0, str.find_first_not_of(" \t\r\n"));
    str.erase(str.find_last_not_of(" \t\r\n") + 1);
    return str;
}
/*
    * str: 待分隔的原始字符串
    * pos: 分隔位置
    * lor: 截取字符串左侧部分(-1) or 右侧部分(1)
    * */
inline string csvFileRead::cutString(string str, int lor)
{
    string s = str;
    int len = 0;
    len = strlen(s.c_str());
    if (lor < 0)
    {
        string ss = s.substr(0, len - 9);
        return ss;
    }
    if (lor > 0)
    {
        string ss = s.substr(len - 9, len);
        return ss;
    }
    return 0;
}

inline float csvFileRead::string_to_float(string str)
{
    int i = 0, len = str.length();
    float sum = 0;
    while (i < len)
    {
        if (str[i] == '.')
            break;
        sum = sum * 10 + str[i] - '0';
        ++i;
    }
    ++i;
    float t = 1, d = 1;
    while (i < len)
    {
        d *= 0.1;
        t = str[i] - '0';
        sum += t * d;
        ++i;
    }
    return sum;
}
bool csvFileRead::read_csv_file(string fileName,vector<LaserMessage> &LaserMsg)
{
    LaserMessage msg;
    ifstream fin(fileName); 
    string line,field;
    vector<string> row; 
    vector<vector<string>> row_column;
    if(fileName.empty())
    {
        cout<<"file name is empty "<<endl;
        return false;
    }
    cout<< "file name:" << fileName << endl;
    if(!fin.is_open())
    {
        cout<<"file name is not exist!!!"<<endl;
        return false;
    }
    while (getline(fin, line))  
    {
        istringstream sin(line);          
        while (getline(sin, field, ','))  
        {
            row.push_back(field);  
        }
        row_column.push_back(row);
        row.clear();
    }
    vector<string> temp_ite;
    vector<vector<string>>::iterator ite = row_column.begin();
    for (ite = ite + 1; ite != row_column.end() - 1; ite++)
    {
        temp_ite = *ite;
        msg.header.seq = atof(temp_ite[1].c_str());
        msg.header.stamp.tv_sec = atof(cutString(temp_ite[2], -1).c_str());
        msg.header.stamp.tv_nsec = atof(cutString(temp_ite[2], 1).c_str());
        msg.header.frame_id = temp_ite[3];
        msg.angle_min = atof(temp_ite[4].c_str());
        msg.angle_max = atof(temp_ite[5].c_str());
        msg.angle_increment = atof(temp_ite[6].c_str());
        msg.time_increment = atof(temp_ite[7].c_str());
        msg.scan_time = atof(temp_ite[8].c_str());
        msg.range_min = atof(temp_ite[9].c_str());
        msg.range_max = atof(temp_ite[10].c_str());
		msg.range_nums = atof(temp_ite[11].c_str());
        for (unsigned int i = 0; i< msg.range_nums; i++)
        {
            msg.ranges.push_back(atof((*ite)[i+12].c_str()));
        }
		for(unsigned int i =(12+msg.range_nums); i< temp_ite.size(); i++)
		{
			msg.angles.push_back(atof((*ite)[i].c_str()));
		}
        LaserMsg.push_back(msg);
        msg.ranges.clear();
		msg.angles.clear();
    }
    cout<< "Read Data succeed." << endl ;
    return true;
}

bool csvFileRead::read_csv_file(string fileName,vector<UlOdomImu_t> &UlOdomImu_t)
{
    struct UlOdomImu_t msg;
    char name[14];
    ifstream fin(fileName);  
    string line,field;
    vector <string> row;  
    vector <vector<string> > row_column;
    if(fileName.empty())
    {
        cout<<"file name is empty "<<endl;
        return false;
    }
    cout<< "file name:" << fileName << endl;
    while (getline(fin, line))  
    {
        istringstream sin(line);          
        while (getline(sin, field, ','))  
        {
            row.push_back(field);  
        }
        row_column.push_back(row);
        row.clear();
    }
    vector<string> temp_ite;
    vector<vector<string> >::iterator ite = row_column.begin();
    for (ite = ite + 1; ite != row_column.end() - 1; ite++)
    {
        temp_ite = *ite;
        msg.header.seq = atof(temp_ite[1].c_str());
        msg.header.stamp.tv_sec = atof(cutString(temp_ite[2], -1).c_str());
        msg.header.stamp.tv_nsec = atof(cutString(temp_ite[2], 1).c_str());
        msg.header.frame_id = temp_ite[3];
        msg.rollAngle= atof(temp_ite[4].c_str());
        msg.pitchAngle = atof(temp_ite[5].c_str());
        msg.yawAngle = atof(temp_ite[6].c_str());
        msg.xGyro = atof(temp_ite[7].c_str());
        msg.yGyro = atof(temp_ite[8].c_str());
        msg.zGyro = atof(temp_ite[9].c_str());
        msg.xAccelerometer = atof(temp_ite[10].c_str());
        msg.yAccelerometer = atof(temp_ite[11].c_str());
        msg.zAccelerometer = atof(temp_ite[12].c_str());
        msg.xMagneticCompass = atof(temp_ite[13].c_str());
        msg.yMagneticCompass = atof(temp_ite[14].c_str());
        msg.zMagneticCompass = atof(temp_ite[15].c_str());
        msg.rWheelEncoder = atof(temp_ite[16].c_str());
        msg.lWheelEncoder = atof(temp_ite[17].c_str());
        msg.xSum = atof(temp_ite[18].c_str());
        msg.ySum = atof(temp_ite[19].c_str());
        UlOdomImu_t.push_back(msg);
    }
    cout<< "Read Data succeed." << endl ;
    return true;
}

bool csvFileRead::write_csv_file(string fileName,vector<UlOdomImu_t> &UlOdomImu_t)
{
    //create file 
    ofstream oFile;
    string csvfilename=fileName+".csv";
    oFile.open(csvfilename,ios::out | ios::trunc);

    for(auto it=UlOdomImu_t.begin();it!=UlOdomImu_t.end();it++)
    {
        oFile<<to_string((*it).header.stamp.tv_sec*1000000000ll+(*it).header.stamp.tv_nsec)<<","
             <<to_string((*it).header.seq)<<","
             <<to_string((*it).header.stamp.tv_sec*1000000000ll+(*it).header.stamp.tv_nsec)<<","
             <<(*it).header.frame_id<<","
             <<to_string((*it).rollAngle)<<","
             <<to_string((*it).pitchAngle)<<","
             <<to_string((*it).yawAngle)<<","
             <<to_string((*it).xGyro)<<","
             <<to_string((*it).yGyro)<<","
             <<to_string((*it).zGyro)<<","
             <<to_string((*it).xAccelerometer)<<","
             <<to_string((*it).yAccelerometer)<<","
             <<to_string((*it).zAccelerometer)<<","
             <<to_string((*it).xMagneticCompass)<<","
             <<to_string((*it).yMagneticCompass)<<","
             <<to_string((*it).zMagneticCompass)<<","
             <<to_string((*it).rWheelEncoder)<<","
             <<to_string((*it).lWheelEncoder)<<","
             <<to_string((*it).xSum)<<","
             <<to_string((*it).ySum)<<","<<endl;
    }
}

